
"""
程序主入口，启动并运行仿真，处理仿真与信号机通讯
"""

from udp import UDP_Client, UDP_Server
from dettlssumo import SUMO
from junction import Junction
import config
import queuedata
import threading
import struct
import datetime
import xml.etree.ElementTree as ET
class Main:
    def __init__(self):
        self.scenario_path = './scenario/'  # 指定场景路径(可改为从命令行参数获取)
        self.init_param()
        self.init_devices()
        self.init_junctions()
        self.init_tls_server()
        self.init_flow_client()
        self.init_sumo()



    def init_param(self):
        from configparser import ConfigParser
        cfg = ConfigParser()
        cfg.read(self.scenario_path+'param.ini', encoding='utf-8')
        config.sumoCfg = cfg.get('SUMO', 'sumoCfg')
        config.sumoNet = cfg.get('SUMO', 'sumoNet')
        config.sumoAdd = cfg.get('SUMO', 'sumoAdd')
        config.sumoPath = cfg.get('SUMO', 'sumoPath')
        config.deviceCfg = cfg.get('OTHER', 'deviceCfg')
        config.steplength = cfg.getfloat('SUMO', 'steplength')
        config.multiple = cfg.getint('SUMO', 'multiple')
        config.cinductionloopid = cfg.get('SUMO', 'cinductionloopid')
        config.radius = cfg.getfloat('SUMO', 'radius')
        config.tlsport = cfg.getint('SUMO', 'tlsport')
        config.simulationDuration = cfg.getint('SUMO', 'simulationDuration')

    def init_devices(self):
        # JunctionID,DeviceID,IP,Port,LinksNum,Channels
        # 0         ,1       ,2 ,3   ,4       ,5
        # str       ,int     ,str,int,int     ,dict
        import csv
        with open(self.scenario_path + config.deviceCfg, 'r') as fp:
            reader = csv.reader(fp)
            next(reader)  # 跳过标题
            for row in reader:
                if '' not in row:  # 跳过配置信息不完整的行
                    config.devices[row[0]] = int(row[1])
                    config.devices2junction[int(row[1])] = row[0]
                    config.addrs[row[0]] = (row[2], int(row[3]))
                    config.junction_groupnum[row[0]] = int(row[4])  # 路口id对应的通道数量，与信号机发送的灯色个数一致

    def init_junctions(self):
        # 注意修改为实际使用场景的路网net文件名
        tree = ET.parse(config.sumoPath + config.sumoNet)
        root = tree.getroot()
        for item in root.findall('junction'):
            if item.attrib['type'] == 'traffic_light':
                junctionid = item.attrib['id']
                if junctionid in config.devices.keys():  # 仅初始化有设备的路口
                    config.junctions.append(junctionid)
                    config.junction_lanes[junctionid] = item.attrib['incLanes'].split()

        # 注意修改为实际使用场景的检测器det文件名
        tree2 = ET.parse(config.sumoPath + config.sumoAdd)
        root2 = tree2.getroot()
        for k, v in config.junction_lanes.items():
            config.junction_detectors[k] = list()
            for det in root2.findall('inductionLoop'):
                if det.attrib['lane'] in v:
                    config.junction_detectors[k].append(det.attrib['id'])

        for v in config.junction_detectors.values():  # 将检测器id按添加顺序编号排序 发送的车检器数据串依此顺序
            v.sort(key=lambda e: int(e.split('_')[-1]))


    def init_tls_server(self):
        self.tlsudpser = UDP_Server(config.tlsport, queuedata.tls_queue)

    def init_sumo(self):
        SUMO.junctions = dict()
        for jid in config.junctions:
            SUMO.junctions[jid] = Junction(jid)
        self.sumo = SUMO(config.sumoPath+config.sumoCfg)
        self.sumo.subInductionloop(config.cinductionloopid, config.radius)

    def start_sumo(self):
        self.sumo.simulation()

    def init_flow_client(self):
        self.flowUdpClient = UDP_Client(queuedata.det_queue)

    def process_detqueue(self):

        contrast_list = []
        # 车检器消息
        message_start = 0xc0  # 消息头
        editon = 0x10  # 版本号
        sender = 0x80  # 发送方标识
        receiver = 0x10  # 接收方标识
        data_link_code2 = 0x02  # 交通流信息数据链路码
        region = 0x00  # 区域号
        jun_N = 0x00  # 路口号
        operation_type = 0x82  # 车检器实时检测信息操作类型
        object_identification = 0x51  # 车检器实时消息对象标识
        reserve = 0x00
        content_format = 0x01  # 车检器实时检测信息内容格式
        device1_ip1 = 192  # 车检器实时检测信息ip，字节1
        device1_ip2 = 168  # 车检器实时检测信息ip，字节2
        device1_ip3 = 14  # 车检器实时检测信息ip，字节3
        device1_ip4 = 168  # 车检器实时检测信息ip，字节4
        coil_id = 0x00  # 线圈编号
        message_end = 0xc0  # 报文尾
        # 联机心跳
        data_link_code1 = 0x01  # 心跳数据链路码
        operation_type1 = 0x80  # 心跳操作类型
        object_identification1 = 0x01  # 心跳对象标识
        # 交通流消息
        object_identification2 = 0x02  # 交通流信息对象标识
        content_format2 = 0x01  # 交通流信息内容格式
        ssss = 0
        H = 0  # 心跳及交通流发送计时
        while True:
                detdata = queuedata.det_queue.get()
                H += 1
                if len(contrast_list) < 2:  # 对比前后两组数据
                    contrast_list.append(detdata)
                else:
                    del contrast_list[0]
                    contrast_list.append(detdata)
                for junction in config.junctions:
                    device1_ip1 = int(junction[0])
                    device1_ip2 = int(junction[1])
                    device1_ip3 = int(junction[2])
                    device1_ip4 = int(junction[3:])
                    J = Junction(junction)
                    # print(J.detectors)
                    # 车检器实时检测信息
                    if len(contrast_list) == 2:
                        # print(contrast_list)
                        try:
                            for i in contrast_list[0]:
                                if i in J.detectors:
                                    if 16 in contrast_list[0][i] and 16 in contrast_list[1][i] and contrast_list[0][i][16] != contrast_list[1][i][16]:
                                        IO_stage = int('{:#04x}'.format(contrast_list[1][i][16]), base=16)
                                        coil_id = int('{:#04x}'.format(int(''.join(i.split('_')[-1:]))), base=16)
                                        # print(coil_id, IO_stage)
                                        message_check = (sum([editon, sender, receiver,
                                                            data_link_code2, region, jun_N, operation_type,
                                                            object_identification, reserve,
                                                            reserve, reserve, content_format, device1_ip1,
                                                            device1_ip2, device1_ip3, device1_ip4,
                                                            coil_id, IO_stage])) % 256

                                        bindet = struct.pack('BBBBBBHBBHHBBBBBBBBBB', message_start, editon, sender,
                                                            receiver,
                                                            data_link_code2, region, jun_N, operation_type,
                                                            object_identification, reserve,
                                                            reserve, reserve, content_format, device1_ip1, device1_ip2,
                                                            device1_ip3, device1_ip4,
                                                            coil_id, IO_stage, message_check, message_end)
                                        # print(J.addr, bindet)
                                        self.flowUdpClient.send(J.addr, bindet)
                        except Exception as e:
                            #print('contrast_list:', contrast_list)
                            pass
                                    
                # 联机心跳消息
                if H == 5/config.steplength:
                    for junction in config.junctions:
                        J = Junction(junction)
                        message_check_Heart = (sum([editon, sender, receiver,
                                                    data_link_code1, region, jun_N, operation_type,
                                                    object_identification1,
                                                    reserve,
                                                    reserve, reserve, content_format])) % 256

                        heartBeat = struct.pack('BBBBBBHBBHHBBBB', message_start, editon, sender,
                                                receiver,
                                                data_link_code1, region, jun_N, operation_type,
                                                object_identification1,
                                                reserve,
                                                reserve, reserve, content_format, message_check_Heart,
                                                message_end)
                        # print(J.addr, heartBeat)
                        self.flowUdpClient.send(J.addr, heartBeat)
                        # 发送交通流信息
                        coilNumber = 0
                        detList = []
                        alLDetNum = 0
                        fmtStr = ''
                        v = []
                        try:
                            for everyDetector in detdata:
                                if everyDetector in J.detectors:
                                    coilNumber += 1
                                    detList.append((int(''.join(everyDetector.split('_')[-1:])), detdata[everyDetector][16], 0, 0))
                        except Exception as e:
                            print('Exception in detdata:', detdata)
                        
                        for trafficFLowDet in detList:
                            alLDetNum += (trafficFLowDet[0] + trafficFLowDet[1])
                        # print(detList)
                        trafficMessageCheck = (sum([editon, sender, receiver,
                                                    data_link_code2, region, jun_N, operation_type,
                                                    object_identification2, reserve,
                                                    reserve, reserve, content_format,
                                                    device1_ip1, device1_ip2, device1_ip3, device1_ip4,
                                                    coilNumber, alLDetNum])) % 256
                        for detTuple in detList:
                            fmtStr += 'BBBB'
                            for everyValue in detTuple:
                                v.append(everyValue)
                        trafficFlowMessage = struct.pack('BBBBBBHBBHHBBBBBBB' + fmtStr + 'BB', message_start,
                                                         editon, sender,
                                                         receiver, data_link_code2, region, jun_N, operation_type,
                                                         object_identification2, reserve, reserve, reserve,
                                                         content_format, device1_ip1, device1_ip2, device1_ip3,
                                                         device1_ip4,
                                                         coilNumber, *v,
                                                         trafficMessageCheck, message_end)
                        # print(J.addr, trafficFlowMessage)
                        self.flowUdpClient.send(J.addr, trafficFlowMessage)
                    H = 0

    def process_tlsqueue_grouped(self):
        """
        处理接收灯色数据，基于分组
        """
        mask1 = 0b11110000
        mask = 0b00001111  # 通过与运算先解码后四位(半个字节)，然后通过移位运算解码前四位
        bit2ryg = {0: '0', 1: 'r', 2: 'y', 3: 'G', 4: 'G', 5: 'o'}
        l = [[111]]
        while True:
                try:
                    tlsbin = queuedata.tls_queue.get()
                    # print(tlsbin)
                    if len(l) < 2:  # 去重
                        l.append(tlsbin)
                    else:
                        del l[0]
                        l.append(tlsbin)
                    if l[0] != l[1]:
                        # print(tlsbin)
                        tlsinfo = struct.unpack('H' + 'B' * (len(tlsbin) - 2), tlsbin)  # '>H'表示大端序解析两个字节 'B'*(len(tlsbin)-2)表示剩余的字节，每一个字节解析一次,##42个字节，前2个字节为id,后面每一个字节代表两个通道
                        cstatus = []
                        for b in tlsinfo[1:]:
                            c = b & mask
                            cstatus.append(b >> 4)    # 然后解析前四位，移位运算并不影响list表中的值
                            cstatus.append(c)
                        # print(tlsinfo[0])
                        if str(tlsinfo[0]) in config.junctions:  # 仅处理关注的路口
                            junctionid = config.devices2junction[tlsinfo[0]]
                            tlsid = SUMO.junctions[junctionid].tlLogicid      # 信号id
                            groupNum = SUMO.junctions[junctionid].groupnum  # 路口对应的通道数量
                            ryg = [bit2ryg[cstatus[i]] for i in range(groupNum)]
                            rygstr = ''.join(ryg)
                            # print(tlsid, rygstr)
                            # print(lightStateOfPhase)
                            queuedata.ryg_queue.put((tlsid, rygstr))
                except Exception as e:
                    print(e)


    def process_tlsqueue(self):
        """
            处理接收的灯色数据
        """
        mask1 = 0b11110000
        mask2 = 0b00001111
        #          关灯    红      黄       绿      绿闪    黄闪
        bit2ryg = {0: 'O', 1: 'r', 2: 'y', 3: 'g', 4: 'g', 5: 'o'}
        while True:
                try:
                    tlsbin = queuedata.tls_queue.get()
                    #print(len(tlsbin), ' ', tlsbin)
                    tlsinfo = struct.unpack('>H'+'B'*(len(tlsbin)-2), tlsbin)
                    cstatus = list()
                    for b in tlsinfo[1:]:
                        cstatus.append((b & mask1) >> 4)
                        cstatus.append(b & mask2)
                    if str(tlsinfo[0]) in config.junctions:  # 仅处理关注的路口
                        #print(tlsinfo[0], cstatus)
                        junctionid = config.devices2junction[tlsinfo[0]]
                        tlsid = SUMO.junctions[junctionid].tlLogicid
                        channels = SUMO.junctions[junctionid].channels
                        ryg = ['g']*SUMO.junctions[junctionid].linksnum
                        # print('tlsid = ', tlsid, '\n channels = ', channels)
                        for c, l in channels.items():
                            for link in l:
                                ryg[link-1] = bit2ryg[cstatus[c-1]]
                        rygstr = ''.join(ryg)
                        # print(tlsid, rygstr)
                        queuedata.ryg_queue.put((tlsid, rygstr))

                except Exception as e:
                    print('=================================')
                    print(e)


    def getLocalTime(self):
        # now = time.strftime('%Y-%m-%d %H:%H:%S',time.localtime(time.time()))
        now = datetime.datetime.now()
        now = str(now)[:-3]
        return now



if __name__ == "__main__":
    main = Main()
    threads = []
    process_detqueue_thread = threading.Thread(target=main.process_detqueue)
    tls_server_thread = threading.Thread(target=main.tlsudpser.run)
    process_tlsqueue_thread = threading.Thread(target=main.process_tlsqueue_grouped)
    threads.append(process_detqueue_thread)
    threads.append(tls_server_thread)
    threads.append(process_tlsqueue_thread)
    for thread in threads:
        thread.start()
    main.start_sumo()
